#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>

/// @brief 标准PID结构体
typedef struct
{
    float Kp; ///< 比例系数
    float Ki; ///< 积分系数
    float Kd; ///< 微分系数

    float SetPoint;     ///< 目标值
    float ProcessValue; ///< 过程值
    float Output;       ///< 输出值
    float Error;        ///< 误差值
    float LastError;    ///< 上一次误差值
    float Integral;     ///< 积分值
    float Derivative;   ///< 微分值
} Standard_Pid_t;

Standard_Pid_t Standard_Pid = {0};

#define CACHE_SZIE 4096

////////////////////////////////////////////////////////////////////////

/// @brief 日志输出缓冲区数据
/// @param data
/// @param len
void Printf_Buf(uint8_t *data, uint16_t len)
{
    // 十六进制显示
    for (int i = 0; i < len; i++)
    {
        if (i % 16 == 0 && i != 0)
        {
            printf("\n");
        }

        printf("%02X ", data[i]);
    }
    printf("\n");

    // ASCII码显示
    for (int i = 0; i < len; i++)
    {
        printf("%c", data[i]);
    }
    printf("\n");
}

/// @brief PID 计算函数
/// @param pid
/// @param process_value
/// @return
void Pid_Calculate(Standard_Pid_t *pid, float process_value)
{
    if (pid == NULL)
    {
        return; // 检查PID结构体是否为空
    }

    // 更新过程值
    pid->ProcessValue = process_value;

    // 计算误差
    pid->Error = pid->SetPoint - pid->ProcessValue;

    // 计算积分
    pid->Integral += pid->Error;

    // 计算微分
    pid->Derivative = (pid->Error - pid->LastError);

    // 计算输出值
    pid->Output = pid->Kp * pid->Error +
                  pid->Ki * pid->Integral +
                  pid->Kd * pid->Derivative;

    // 更新上一次误差值
    pid->LastError = pid->Error;
}

/// @brief 写入数据到CSV文件
/// @param filename
/// @param data
/// @param len
void Write_Csv_File(const char *filename, uint8_t *data, uint32_t len)
{
    uint8_t FileName[256] = {0};
    uint16_t FileNameLen = 0;

    // 文件地址检测
    if (filename == NULL || data == NULL || len <= 0)
    {
        printf("Invalid parameters for Write_Csv_File\n");
        return;
    }

    FileNameLen = strlen(filename) - 4;
    memcpy(FileName, filename, FileNameLen); // 复制文件名到输出文件名
    strcat(FileName, "_out.csv");            // 添加 "_out.csv" 后缀

    // 打开文件
    FILE *file = fopen(FileName, "aw+");
    if (file == NULL)
    {
        printf("Failed to open file: %s\n", FileName);
        return;
    }

    //  写入数据到文件
    size_t written = fwrite(data, 1, len, file);
    if (written != len)
    {
        printf("Failed to write all data to file: %s\n", FileName);
    }

    // 关闭文件
    fclose(file);
}

/// @brief 读取CSV文件数据
/// @param filename
/// @param pid
void Read_Csv_File(const char *filename, Standard_Pid_t *pid)
{
    // 文件地址检测
    if (filename == NULL)
    {
        printf("Filename is NULL\n");
        return;
    }

    // 打开文件
    FILE *file = fopen(filename, "r");
    if (file == NULL)
    {
        printf("Failed to open file: %s\n", filename);
        return;
    }

    /*
        csv 文件内容示例：
        1.0,0.3
        1.1,0.2
        1.5,0.55
        1.555,3.497

    */

    uint32_t WriteLen = 0;                              // 写入长度
    uint8_t *FileCache = (uint8_t *)malloc(CACHE_SZIE); // 分配内存用于缓存文件内容
    if (FileCache == NULL)
    {
        printf("Failed to allocate memory for file cache\n");
        fclose(file);
        return;
    }

    // 清空缓存内存
    memset(FileCache, 0, CACHE_SZIE);

    // 文件表头
    sprintf((char *)FileCache, "Input,Output,CalcValue"); // 写入表头
    WriteLen = strlen((char *)FileCache);                 // 获取写入长度

    // 读取 CSV 文件数据
    uint8_t line[128] = {0},
            LineBuf[128] = {0};
    float Cache[2] = {0};
    uint16_t LineCount = 0;
    while (fgets(line, sizeof(line), file) != NULL)
    {
        memset(Cache, 0, sizeof(Cache));     // 清空缓存数据
        memset(LineBuf, 0, sizeof(LineBuf)); // 清空行缓冲区

        // 获取数据
        sscanf(line, "%f,%f", &Cache[0], &Cache[1]);

        // PID 计算
        Pid_Calculate(pid, Cache[1]); // 计算 PID 输出

        // 格式化输出数据
        sprintf((char *)LineBuf, "%f,%f,%f", Cache[1], pid->Output, Cache[1] + pid->Output);

        // 将计算结果写入缓存
        sprintf((char *)FileCache + WriteLen, "\n%s", LineBuf);

        // 获取写入长度
        WriteLen += strlen((char *)LineBuf);

        // 到临界状态，先写入一次文件，再清空缓存
        if (WriteLen >= CACHE_SZIE - 128)
        {
            // 写入 CSV 文件数据
            Write_Csv_File(filename, FileCache, WriteLen);

            // 清空缓存内存
            memset(FileCache, 0, CACHE_SZIE); // 清空缓存内存
            WriteLen = 0;                     // 重置写入长度为0
        }

        LineCount++;
    }

    // 写入剩余数据到文件
    if (WriteLen > 0)
    {
        // 写入 CSV 文件数据
        Write_Csv_File(filename, FileCache, WriteLen);
    }

    // 释放缓存内存
    if (FileCache != NULL)
    {
        free(FileCache);
        FileCache = NULL;
    }
    printf("Total lines read: %d\n", LineCount); // 输出读取行数
}

/// @brief 初始化 PID 参数
/// @param pid
void Pid_Init(Standard_Pid_t *pid)
{
    // 清空结构体内存
    memset(pid, 0, sizeof(Standard_Pid_t)); // 清零结构体内存

    // 设置 PID 初始参数
    pid->Kp = 1.0;
    pid->Ki = 0.1;
    pid->Kd = 0.001;
}

/// @brief 清空文件内容
/// @param filename
void Clear_Csv_File(const char *filename)
{
    //  文件名数组 1 读取文件名 2 写入文件名
    uint8_t FileName[2][256] = {0};
    uint16_t FileNameLen = 0;

    if (filename == NULL)
    {
        printf("Filename is NULL\n");
        return;
    }

    // PID 输入数据文件
    strcpy(FileName[0], filename);

    // PID 输出数据文件

    FileNameLen = strlen(filename) - 4;
    memcpy(FileName[1], filename, FileNameLen); // 复制文件名到输出文件名
    strcat(FileName[1], "_out.csv");            // 添加 "_out.csv" 后缀

    // 清空文件内容
    for (uint8_t i = 1; i < 2; i++)
    {
        // 打开文件
        FILE *file = fopen(FileName[i], "w");
        if (file == NULL)
        {
            printf("Failed to open file: %s\n", FileName[i]);
            return;
        }

        // 清空文件内容
        if (fprintf(file, "") < 0)
        {
            printf("Failed to clear file: %s\n", FileName[i]);
        }

        // 关闭文件夹
        fclose(file);
    }
}

/// @brief 输出命令行参数
/// @param argc
/// @param argv
void Param_Output(int argc, char *argv[])
{
    for (int i = 0; i < argc; i++)
    {
        printf("argv[%d] = %s\n", i, argv[i]);
    }
}

int main(int argc, char *argv[])
{

    Param_Output(argc, argv);

    if (argc < 6)
    {
        // filename  Kp Ki Kd
        printf("Usage: %s <filename> <Std> <Kp> <Ki> <Kd> \n", argv[0]);
        return 0;
    }

    // 清空文件内容
    Clear_Csv_File(argv[1]);

    // 初始化 PID 参数
    Pid_Init(&Standard_Pid);
    Standard_Pid.SetPoint = atof(argv[2]); // 设置目标值
    Standard_Pid.Kp = atof(argv[3]);       // 读取 Kp 值
    Standard_Pid.Ki = atof(argv[4]);       // 读取 Ki 值
    Standard_Pid.Kd = atof(argv[5]);       // 读取 Kd 值

    // 读取 CSV 文件数据
    Read_Csv_File(argv[1], &Standard_Pid);

    return 0;
}